//============================================================================
// Name        : pcl2_transformer.cpp
// Author      :
// Version     :
// Description :
//============================================================================

#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <boost/foreach.hpp>
#include <pcl_ros/transforms.h>

class My_pcl2_transformer {
public:
	My_pcl2_transformer();
	void scanCallback(const sensor_msgs::PointCloud2::ConstPtr& scan);
private:
	ros::NodeHandle node_;
	ros::Subscriber sub_;
	ros::Publisher point_cloud_publisher_;
	tf::Transform tf_transform_;
};

My_pcl2_transformer::My_pcl2_transformer(){

	sub_ = node_.subscribe<sensor_msgs::PointCloud2>("/StereoMatcher/pointCloud", 10, &My_pcl2_transformer::scanCallback, this);
	point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud2", 10, false);
	tf_transform_.setOrigin(tf::Vector3(0, 0, 1.56));
	tf_transform_.setRotation(tf::createQuaternionFromRPY(-1.34, 0, 1.57));
}

void My_pcl2_transformer::scanCallback(const sensor_msgs::PointCloud2::ConstPtr& scan){
	sensor_msgs::PointCloud2 cloud;

	pcl_ros::transformPointCloud("/stereo", tf_transform_, *scan, cloud);

	//	cloud = *scan;
	//	cloud.header.frame_id = "/laser";

	point_cloud_publisher_.publish(cloud);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "my_pcl2_transformer");

	My_pcl2_transformer pcl2_transformer;

	ros::spin();

	return 0;
}
